/**
 * This file is part of the nestk library.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 * Author: Nicolas Burrus <nicolas.burrus@uc3m.es>, (C) 2010
 */

#include "rgbd_processor.h"
#include <ntk/utils/opencv_utils.h>
#include <ntk/utils/time.h>
#include <ntk/geometry/pose_3d.h>
#include <map>
#include <queue>
using namespace cv;

namespace ntk
{

	RGBDProcessor :: RGBDProcessor() :
            m_image(0),
            //m_flags(FixGeometry | FixBias | UndistortImages | ComputeNormals),
            m_flags(UndistortImages),
            m_min_depth(0.05),
            m_max_depth(6.0),
            m_max_normal_angle(80),
            m_max_time_depth_delta(0.1),
            m_max_spatial_depth_delta(0.1),
            m_mapping_resolution(1.0),
            m_min_amplitude(1000),
            m_max_amplitude(-1),
			m_modified(true),
			m_parts_segment_threshold(0.02),
			m_parts_count_threshold(400),
			m_image_features_estimation(false),
			m_image_features_detector_type("SIFT"),
			m_image_features_descriptor_type("SIFT")
    {
    }

    void RGBDProcessor :: fixDepthGeometry()
    {
        ntk_ensure(m_image->calibration(), "Calibration required.");
        double cx = m_image->calibration()->depth_intrinsics(0,2);
        double cy = m_image->calibration()->depth_intrinsics(1,2);
        double fx = m_image->calibration()->depth_intrinsics(0,0);
        double fy = m_image->calibration()->depth_intrinsics(1,1);

        cv::Mat1f& depth_im = m_image->depthRef();
        for_all_rc(depth_im)
        {
            double orig_depth = depth_im(r,c);

            double dx = c-cx;
            double dy = r-cy;

            Point3f v(dx/fx, dy/fy, 1);
            double norm = sqrt(v.dot(v));
            v = v * (1.0/norm);
            v *= orig_depth;

            double depth_z = v.z;
            depth_im(r,c) = depth_z;
        }
    }

    void RGBDProcessor :: fixDepthBias()
    {
        ntk_assert(0, "not updated.");
#if 0 // FIXME: obsolete
        cv::Mat1f& depth_im = m_image->depthRef();
        for_all_rc(depth_im)
        {
            double depth_z = depth_im(r,c);

            // depth correction is accurate only in the 30cm-110cm range.
            double x = depth_z;
            if (depth_z < 0.3) x = 0.3;
            if (depth_z > 0.8) x = 0.8;
            double offset =   m_image->calibration()->depth_correction(0,4)*x*x*x*x
                              + m_image->calibration()->depth_correction(0,3)*x*x*x
                              + m_image->calibration()->depth_correction(0,2)*x*x
                              + m_image->calibration()->depth_correction(0,1)*x
                              + m_image->calibration()->depth_correction(0,0);

            depth_im(r,c) = depth_z + offset;
        }
#endif
    }

    // FIXME: why is it so slow ?
    void RGBDProcessor :: computeNormals()
    {
        ntk_ensure(m_image->calibration(), "Calibration required.");
        const Pose3D& depth_pose = *m_image->calibration()->depth_pose;
        const cv::Mat1f& depth_im = m_image->depth();
        cv::Mat3f& normal_im = m_image->normalRef();
        normal_im = cv::Mat3f(depth_im.size());
        normal_im = Vec3f(0,0,0);
        for_all_rc(depth_im)
        {
            normal_im(r,c) = estimate_normal_from_depth(depth_im, depth_pose, r, c);
        }
    }

    void RGBDProcessor :: processImage(RGBDImage& image)
    {
        m_image = &image;

        if (m_flags & FlipColorImage)
        {
            flip(m_image->rawRgb(), m_image->rawRgbRef(), -1);
        }

        if (!(m_flags&NiteProcessed) && m_image->rawIntensity().data)
        {
            normalize(m_image->rawIntensityRef(), m_image->rawIntensityRef(), 0, 255, NORM_MINMAX, -1);
        }

        if ((m_flags&NiteProcessed) || (!m_image->calibration() || !(m_flags & UndistortImages) || (m_flags & Pause)))
        {
            m_image->rawAmplitude().copyTo(m_image->amplitudeRef());
            m_image->rawIntensity().copyTo(m_image->intensityRef());
            m_image->rawDepth().copyTo(m_image->depthRef());
            m_image->rawRgb().copyTo(m_image->rgbRef());
        }

        TimeCount tc_undistort("undistort", 2);
        if (!(m_flags&NiteProcessed) && m_image->calibration() && (m_flags & UndistortImages))
            undistortImages();
        tc_undistort.stop();

        TimeCount tc_depth("compute_depth", 2);
        if (m_flags & ComputeKinectDepthLinear)
            computeKinectDepthLinear();
        else if (m_flags & ComputeKinectDepthTanh)
            computeKinectDepthTanh();
        else if (m_flags & ComputeKinectDepthBaseline)
            computeKinectDepthBaseline();
        tc_depth.stop();

        m_image->depthMaskRef() = cv::Mat1b(m_image->rawDepth().size());
        for_all_rc(m_image->depthMaskRef())
        {
            float d = m_image->depth()(r,c);
            if (d < 1e-5 || ntk::math::isnan(d) || d < m_min_depth || d > m_max_depth)
                m_image->depthMaskRef()(r,c) = 0;
            else
                m_image->depthMaskRef()(r,c) = 255;
        }
		m_image->depthMask().copyTo(m_image->mappedDepthMaskRef()); //notice here the depth mask modified again!
		m_image->depthMask().copyTo(m_image->rgbMaskRef());

        if (m_image->calibration())
        {
            if (m_flags & FixGeometry)
                fixDepthGeometry();

            if (m_flags & FixBias)
                fixDepthBias();

            if (m_flags & FilterMedian)
                medianFilter();

            if ((m_flags & ComputeNormals) || (m_flags & FilterNormals))
                computeNormals();

            if (m_flags & FilterThresholdDepth)
                applyDepthThreshold();

            if (m_flags & FilterAmplitude)
                removeLowAmplitudeOutliers();

            if (m_flags & FilterEdges)
                removeEdgeOutliers();

            if (m_flags & FilterNormals)
                removeNormalOutliers();

            if (m_flags & FilterUnstable)
                removeUnstalbePixels();// removeUnstableOutliers(); modified!  

            if (m_flags & PartsSegment)
				partsSegmentation();
			
			if (m_flags & RemoveSmallStructures)
                removeSmallStructures();

            if (m_flags & FillSmallHoles)
                fillSmallHoles();

			 if (m_flags & ComputeMapping)
                computeMappings();
			 
			 if (getImageFeaturesEstimation())
				imageFeaturesEstimation();
        }

        if (m_image->rgb().data)
            cvtColor(m_image->rgb(), m_image->rgbAsGrayRef(), CV_BGR2GRAY);
        int64 tfinal = ntk::Time::getMillisecondCounter();
    }

    void RGBDProcessor :: undistortImages()
    {
        cv::Mat3b tmp3b;
        cv::Mat1f tmp1f;

        cv::Mat3b& rgb_im = m_image->rgbRef();
        const cv::Mat3b& raw_rgb_im = m_image->rawRgb();

        if (m_image->calibration()->rgbSize() != m_image->calibration()->rawRgbSize())
        {
            // First cut color image to the undistorted image size (not used for kinect).
            cv::Size rgb_size = m_image->calibration()->rgbSize();
            cv::Mat roi = raw_rgb_im(cv::Rect((raw_rgb_im.cols-rgb_size.width)/2.0,
                                              (raw_rgb_im.rows-rgb_size.height)/2.0,
                                              rgb_size.width,
                                              rgb_size.height));
            roi.copyTo(rgb_im);
        }
        else
        {
            raw_rgb_im.copyTo(rgb_im);
        }

        if (!m_image->calibration()->zero_rgb_distortion)
        {
            remap(rgb_im, tmp3b,
                  m_image->calibration()->rgb_undistort_map1,
                  m_image->calibration()->rgb_undistort_map2,
                  CV_INTER_LINEAR);
            tmp3b.copyTo(rgb_im);
        }

        if (!m_image->calibration()->zero_depth_distortion)
        {
            remap(m_image->rawDepthRef(), m_image->depthRef(),
                  m_image->calibration()->depth_undistort_map1,
                  m_image->calibration()->depth_undistort_map2,
                  CV_INTER_LINEAR);
        }
        else
        {
            m_image->rawDepthRef().copyTo(m_image->depthRef());
        }

        if (m_image->calibration()->zero_depth_distortion ||
            (m_flags & NoAmplitudeIntensityUndistort))
        {
            m_image->rawAmplitudeRef().copyTo(m_image->amplitudeRef());
            m_image->rawIntensityRef().copyTo(m_image->intensityRef());
        }
        else
        {
            remap(m_image->rawAmplitudeRef(), m_image->amplitudeRef(),
                  m_image->calibration()->depth_undistort_map1,
                  m_image->calibration()->depth_undistort_map2,
                  CV_INTER_LINEAR);

            remap(m_image->rawIntensityRef(), m_image->intensityRef(),
                  m_image->calibration()->depth_undistort_map1,
                  m_image->calibration()->depth_undistort_map2,
                  CV_INTER_LINEAR);
        }
    }

    void RGBDProcessor :: computeMappings()
    {
        ntk_ensure(m_image->calibration(), "Calibration required.");

        cv::Mat1b& mask_im = m_image->depthMaskRef();

        const Pose3D& depth_pose = *m_image->calibration()->depth_pose;
        const Pose3D& rgb_pose = *m_image->calibration()->rgb_pose;

        const cv::Mat1f& depth_im = m_image->depth();
        const cv::Mat3b& rgb_im = m_image->rgb();

        cv::Mat1f& mapped_depth = m_image->mappedDepthRef();
        mapped_depth = cv::Mat1f(m_image->rgb().size());
        mapped_depth = 0.f;

        cv::Mat3b& mapped_color = m_image->mappedRgbRef();
        mapped_color = cv::Mat3b(m_image->depth().size());

        float delta = 1.0 / m_mapping_resolution;
        for (float r = 0; r < depth_im.rows; r += delta )
            for (float c = 0; c < depth_im.cols; c += delta )
            {
            int i_r = ntk::math::rnd(r);
            int i_c = ntk::math::rnd(c);
            if (!is_yx_in_range(depth_im, i_r, i_c))
                continue;

            if (!mask_im(i_r, i_c))
                continue;

            double depth = depth_im(i_r,i_c);
            cv::Point3f p = depth_pose.unprojectFromImage(Point2f(c,r), depth);
            cv::Point3f prgb = rgb_pose.projectToImage(p);

            int i_y = ntk::math::rnd(prgb.y);
            int i_x = ntk::math::rnd(prgb.x);
            if (is_yx_in_range(rgb_im, i_y, i_x))
            {
                Vec3b bgr = rgb_im(i_y, i_x);
                mapped_color(i_r, i_c) = bgr;
                mapped_depth(i_y, i_x) = prgb.z;
            }
        }
    }

    void RGBDProcessor :: medianFilter()
    {
        medianBlur(m_image->depthRef(), m_image->depthRef(), 3);
    }

#if 0
    void RGBDProcessor :: removeLowAmplitudeOutliers()
    {
        ntk_ensure(m_image->calibration(), "Calibration required.");
        cv::Mat1b& mask_im = m_image->depthMaskRef();
        const cv::Mat1f& depth_im = m_image->depth();
        double amplitude_sum = sum(m_image->amplitude())[0];

        for_all_rc(depth_im)
        {
            if (!mask_im(r,c)) continue;
            double da = m_image->amplitude()(r,c) / amplitude_sum;
            if (da < 0.02)
                mask_im(r,c) = 0;
        }
    }
#else
    void RGBDProcessor :: removeLowAmplitudeOutliers()
    {
        cv::Mat1b& mask_im = m_image->depthMaskRef();
        const cv::Mat1f& depth_im = m_image->depth();

        for_all_rc(depth_im)
        {
            if (!mask_im(r,c)) continue;
            double a = m_image->amplitude()(r,c);

            bool keep_it = true;
            keep_it &= (m_min_amplitude < 0 || a > m_min_amplitude);
            keep_it &= (m_max_amplitude < 0 || a < m_max_amplitude);

            if (!keep_it)
                mask_im(r,c) = 0;
        }
    }
#endif

    void RGBDProcessor :: removeNormalOutliers()
    {
        ntk_ensure(m_image->calibration(), "Calibration required.");
        const Pose3D& depth_pose = *m_image->calibration()->depth_pose;
        cv::Mat1b& mask_im = m_image->depthMaskRef();
        const cv::Mat1f& depth_im = m_image->depth();

        for_all_rc(depth_im)
        {
            if (!mask_im(r,c)) continue;
            Vec3f eyev = camera_eye_vector(depth_pose, r, c);
            Vec3f normal = m_image->normal()(r, c);
            double angle = acos(normal.dot(eyev));
            if (angle > (m_max_normal_angle*M_PI/180.0))
                mask_im(r,c) = 0;
        }
    }

    void RGBDProcessor :: removeUnstableOutliers()
    {
        ntk_ensure(m_image->calibration(), "Calibration required.");
        cv::Mat1b& mask_im = m_image->depthMaskRef();
        const cv::Mat1f& depth_im = m_image->depth();

        if (!m_last_depth_image.data)
        {
            m_image->depth().copyTo(m_last_depth_image);
            return;
        }

        for_all_rc(depth_im)
        {
            if (!mask_im(r,c)) continue;
            float diff = std::abs(m_last_depth_image(r,c) - depth_im(r,c));
            if (diff > m_max_time_depth_delta)
                mask_im(r,c) = 0;
        }

        m_image->depth().copyTo(m_last_depth_image);
    }

	void RGBDProcessor::removeUnstalbePixels(float boundary_threshold)
	{
		ntk_ensure(m_image->calibration(), "Calibration required");
		cv::Mat1b& mask_im = m_image->mappedDepthMaskRef(); //not change the original depth mask

		const cv::Mat1f& depth_im = m_image->depth();

		for (int r = 0; r < depth_im.rows; ++r) 
			for (int c = 0; c < depth_im.cols; ++c)
		{
			if(r==0 || r==depth_im.rows-1 || c==0 || c== depth_im.cols-1)
			{
				mask_im(r,c) == 0;
				continue;
			}
		
			if(!mask_im(r,c)) continue;
			
			float diff =0;
			for(int nr=-1;nr<=1;nr++)
				for(int nc=-1;nc<=1;nc++)
				{
					float delta = std::abs(depth_im(r,c)-depth_im(r+nr,c+nc));
					if(delta>diff)
						diff = delta;
				}
			if(flt_geq(diff,boundary_threshold))
				mask_im(r,c) = 0;
		}
		//if(_DEBUG)
		//{
		//	imwrite("mask_org.png",m_image->depthMaskRef()-mask_im);
		//	imwrite("mask_unstable_pixels.png",mask_im);
		//}
	}


    void RGBDProcessor :: removeSmallStructures()
    {
        ntk_ensure(m_image->calibration(), "Calibration required.");
        cv::Mat1b& mask_im = m_image->depthMaskRef();

		// The small structures removal depends on parts segmentation.
		if(!(m_flags & PartsSegment))
			partsSegmentation();



        cv::Mat1b tmp;

        cv::morphologyEx(mask_im, tmp,
                         cv::MORPH_CLOSE,
                         getStructuringElement(cv::MORPH_CROSS,
                                               cv::Size(3,3)));

        cv::morphologyEx(tmp, mask_im,
                         cv::MORPH_OPEN,
                         getStructuringElement(cv::MORPH_CROSS,
                                               cv::Size(3,3)));

    }

    void RGBDProcessor :: fillSmallHoles()
    {
        ntk_ensure(m_image->calibration(), "Calibration required.");
        cv::Mat1b& mask_im = m_image->depthMaskRef();
        cv::Mat1f& depth_im = m_image->depthRef();

        cv::Mat1b new_mask;
        cv::morphologyEx(mask_im, new_mask,
                         cv::MORPH_CLOSE,
                         getStructuringElement(cv::MORPH_RECT,
                                               cv::Size(10,10)));
        cv::Mat1b paint_mask = new_mask - mask_im;
        mask_im = new_mask;

        for_all_rc(mask_im)
        {
            if (mask_im(r,c) && depth_im(r,c) < 1e-5)
                paint_mask(r,c) = 1;
        }

        cv::Mat1b depth8b (depth_im.size());
        for_all_rc(depth8b)
        {
            depth8b(r,c) = 255 * (depth_im(r,c) - m_min_depth) / (m_max_depth - m_min_depth);
        }

        cv::Mat1b painted_depth8b;

        cv::inpaint(depth8b, paint_mask, painted_depth8b, 10, cv::INPAINT_NS);
        imwrite("debug_depth.png", depth8b);
        imwrite("debug_painted_depth.png", painted_depth8b);
        imwrite("debug_paint_mask.png", paint_mask);
        imwrite("debug_depth_mask.png", mask_im);

        for_all_rc(paint_mask)
        {
            if (paint_mask(r,c))
            {
                double d = painted_depth8b(r,c) * (m_max_depth-m_min_depth) / 255.0 + m_min_depth;
                depth_im(r,c) = d;
            }
        }
    }

    void RGBDProcessor :: removeEdgeOutliers()
    {
        ntk_ensure(m_image->calibration(), "Calibration required.");
        cv::Mat1b& mask_im = m_image->depthMaskRef();
        const cv::Mat1f& depth_im = m_image->depth();

        for_all_rc(depth_im)
        {
            if (!mask_im(r,c)) continue;

            if (!is_yx_in_range(depth_im, r+1, c+1))
                continue;
            double diff = std::abs(depth_im(r,c) - depth_im(r,c+1))
                          + std::abs(depth_im(r,c) - depth_im(r+1,c));
            diff /= 2.0;
            if (diff > m_max_spatial_depth_delta)
            {
                mask_im(r,c) = 0;
                continue;
            }
        }
    }

    void RGBDProcessor :: computeKinectDepthTanh()
    {
        const float k1 = 1.1863;
        const float k2 = 2842.5;
        const float k3 = 0.1236;

        cv::Mat1f& depth_im = m_image->depthRef();
        for_all_rc(depth_im)
        {
            float depth = depth_im(r,c);
            if (depth < 2047)
            {
                depth = k3 * tanf(depth/k2 + k1);
            }
            else
                depth = 0;
            depth_im(r,c) = depth;
        }
    }

    void RGBDProcessor :: computeKinectDepthBaseline()
    {
        cv::Mat1f& depth_im = m_image->depthRef();

        double depth_baseline = 7.5e-02;
        double depth_offset = 1090;
        double focal_ir = 580;
        if (m_image->calibration())
        {
            depth_baseline = m_image->calibration()->depth_baseline;
            depth_offset = m_image->calibration()->depth_offset;
            focal_ir = m_image->calibration()->depth_pose->meanFocal();
        }

        for_all_rc(depth_im)
        {
            float raw_depth = depth_im(r,c);
            float depth = 0;
            if (raw_depth < 2047)
            {
                depth = focal_ir * 8.0 * depth_baseline / (depth_offset - raw_depth);
            }
            if (depth < 0)
                depth = 0;
            else if (depth > 10)
                depth = 10;
            depth_im(r,c) = depth;
        }
    }

    void RGBDProcessor :: computeKinectDepthLinear()
    {
        cv::Mat1f& depth_im = m_image->depthRef();
        for_all_rc(depth_im)
        {
            float raw_depth = depth_im(r,c);
            float depth = 0;
            if (raw_depth < 2047)
            {
                depth = 1.0 / (raw_depth * -0.0030711016 + 3.3309495161);
            }
            if (depth < 0)
                depth = 0;
            else if (depth > 10)
                depth = 10;
            depth_im(r,c) = depth;
        }
    }

    void RGBDProcessor :: applyDepthThreshold()
    {
        cv::Mat1b& mask_im = m_image->depthMaskRef();
        const cv::Mat1f& depth_im = m_image->depth();

        for_all_rc(depth_im)
        {
            float depth = depth_im(r,c);
            if (depth < m_min_depth || depth > m_max_depth)
                mask_im(r,c) = 0;
        }
    }

    void compute_color_encoded_depth(const cv::Mat1f& depth_im, cv::Mat3b& color_depth_im,
                                     double* i_min_val, double* i_max_val)
    {
        double min_val, max_val;
        if (i_min_val && i_max_val)
        {
            min_val = *i_min_val;
            max_val = *i_max_val;
        }
        else
        {
            minMaxLoc(depth_im, &min_val, &max_val);
        }

        color_depth_im.create(depth_im.size());
        for (int r = 0; r < depth_im.rows; ++r)
        {
            const float* depth_data = depth_im.ptr<float>(r);
            Vec3b* depth_color_data = color_depth_im.ptr<Vec3b>(r);
            for (int c = 0; c < depth_im.cols; ++c)
            {
                int v = 255*6*(depth_data[c]-min_val)/(max_val-min_val);
                if (v < 0) v = 0;
                unsigned char r,g,b;
                int lb = v & 0xff;
                switch (v / 256) {
                case 0:
                    r = 255;
                    g = 255-lb;
                    b = 255-lb;
                    break;
                case 1:
                    r = 255;
                    g = lb;
                    b = 0;
                    break;
                case 2:
                    r = 255-lb;
                    g = 255;
                    b = 0;
                    break;
                case 3:
                    r = 0;
                    g = 255;
                    b = lb;
                    break;
                case 4:
                    r = 0;
                    g = 255-lb;
                    b = 255;
                    break;
                case 5:
                    r = 0;
                    g = 0;
                    b = 255-lb;
                    break;
                default:
                    r = 0;
                    g = 0;
                    b = 0;
                    break;
                }
                if (v == 0)
                {
                    r = g = b = 0;
                }
                depth_color_data[c] = Vec3b(b,g,r);
            }
        }
    }

    void OpenniRGBDProcessor :: computeMappings()
    {
        Size depth_size = m_image->calibration()->raw_depth_size;
        Size rgb_size = m_image->calibration()->rawRgbSize();

        bool mapping_required = depth_size != rgb_size;

        if (!mapping_required)
        {
            m_image->mappedDepthRef() = m_image->depth();
            m_image->mappedRgbRef() = m_image->rgb();
            m_image->mappedDepthMaskRef() = m_image->depthMask();
        }
        else
        {
            m_image->mappedRgbRef().create(m_image->calibration()->raw_depth_size);
            m_image->mappedDepthRef().create(m_image->calibration()->rawRgbSize());
            m_image->mappedDepthMaskRef().create(m_image->calibration()->rawRgbSize());
        }

        const float ratio = float(rgb_size.width) / depth_size.width;

        Size raw_rgb_size(rgb_size.width/ratio, rgb_size.height/ratio);
        cv::Mat3b raw_mapped_rgb(raw_rgb_size);
        cv::resize(m_image->rawRgb(), raw_mapped_rgb, raw_rgb_size, 0, 0, cv::INTER_LINEAR);
        m_image->mappedRgbRef() = raw_mapped_rgb(Rect(Point(0,0),
                                                      depth_size));

        Size raw_depth_size(depth_size.width*ratio, depth_size.height*ratio);

        cv::Mat1f raw_mapped_depth(raw_depth_size);
        cv::resize(m_image->rawDepth(), raw_mapped_depth, raw_depth_size, 0, 0, INTER_NEAREST);

        cv::Mat1b raw_mapped_depth_mask(raw_depth_size);
        cv::resize(m_image->depthMask(), raw_mapped_depth_mask, raw_depth_size, 0, 0, INTER_NEAREST);

        cv::Mat1f roi_depth = m_image->mappedDepthRef()(Rect(Point(0,0), raw_depth_size));
        raw_mapped_depth.copyTo(roi_depth);

        cv::Mat1b roi_depth_mask = m_image->mappedDepthMaskRef()(Rect(Point(0,0), raw_depth_size));
        raw_mapped_depth_mask.copyTo(roi_depth_mask);
    }

	int RGBDProcessor::partsSegmentation()
	{
		setFilterFlag(PartsSegment,false);

		if(m_image->partSizeCount() > 0)
			m_image->partSizeClear();
		float delta = m_parts_segment_threshold;
		ntk::TimeCount parts_segment_time("parts segmentation",0);
		cv::Size label_size = m_image->depth().size();
		uint label_row = m_image->depth().rows;
		uint label_col = m_image->depth().cols;
		
		cv::Mat1w& part_index_im = m_image->userLabelsRef();
		//already defined in the processImage
		//also expand the boundary points as mask
		part_index_im = cv::Mat1w(label_size);
		part_index_im = 0;

		ushort part_index = 1;
		std::vector<uint> vec_com_count;
		vec_com_count.push_back(0); //initializing for the masked pixels index
		for(uint r=1; r< label_row; r++)
		{
			ushort* label_data = part_index_im.ptr<ushort>(r);
			for(uint c=1; c<label_col; c++)
			{
				if(label_data[c] || !m_image->depthMask()(r,c))
					continue;
				//region growing from label_data[c]
				uint com_count =0;
				std::queue<Point2i> label_set_index;
				label_set_index.push( Point2i(r,c));
				com_count++;
				while(label_set_index.size()!=0)
				{
					//ushort label_cur = label_set_index.front();
					Point2i p_l =  label_set_index.front();
					int r_l = p_l.x; //label_cur/label_col; //row_label
					int c_l = p_l.y; //label_cur%label_col; //column_label
					part_index_im(r_l,c_l) = part_index;//label_index;
					label_set_index.pop();
					//int curSize = label_set_index.size();
					for(int r_d = -1; r_d <=1; r_d++)
					{
						int r_n = r_l+r_d;
						if(r_n <0 || r_n > label_row-1)
							break;
						for(int c_d = -1; c_d <= 1; c_d++)
						{
							int c_n = c_l+c_d;
							if( c_n <0 ||  c_n > label_col)
								break;
					
							if(!m_image->depthMask()(r_n,c_n))
								continue;
							if(part_index_im(r_n,c_n))
								continue;
							if( std::abs(m_image->depth()(r_l,c_l) - m_image->depth()(r_n,c_n)) < delta)
							{
								part_index_im(r_n,c_n) = 65535;
								//label_set_index.push( (uint) r_n*label_col + c_n);
								label_set_index.push(Point2i(r_n,c_n));
								com_count++;
							}
						}
					}
				}
				vec_com_count.push_back(com_count);
				part_index++;	
				if(part_index == 65535)
				{
					MessageBox(NULL,"Error in the part segmentation", "parts",0);
					part_index = 1;
				}
			}
		}
		ntk_dbg_print(vec_com_count.size(),1);

		//remove the small part whose number count is less than m_part_count_threshold
		//recording the mapping for the part_index 
		std::vector<ushort> vec_map_index;
		uint part_posted_count = 0;
		m_image->partSizeAdd(0);
		uint active_count = 0;
		
		for(std::vector<uint>::iterator it = vec_com_count.begin(); it != vec_com_count.end(); it++)
		{
			if(*it<m_parts_count_threshold)
				*it = 0;
			if(*it!=0)
			{
				part_posted_count++;
				vec_map_index.push_back(part_posted_count);
				active_count += *it;
				m_image->partSizeAdd(*it);
			}
			else
				vec_map_index.push_back(0);
			
		}
		uint un_active_count = label_row*label_col - active_count;
		vec_com_count[0] = un_active_count;
		m_image->partSizeAt(0) = un_active_count;
		ntk_dbg_print(m_image->partSizeAt(0),1);
		ntk_dbg_print(m_image->partSizeCount(),1);
		
		//remove the small parts
		//expand the boundary
		
		cv::Mat1b& rgb_mask_im = m_image->rgbMaskRef();

		for(uint r=1; r< label_row; r++)
		{
			ushort* label_data = part_index_im.ptr<ushort>(r);
			uchar*  rgb_mask  = rgb_mask_im.ptr<uchar>(r);
			for(uint c=1; c<label_col; c++)
			{
				
				ushort cur_idx = label_data[c];
				if(!cur_idx)
				{
					rgb_mask[c] =0;
					continue;
				}
				ushort cur_val = vec_map_index[cur_idx];
				label_data[c] = cur_val;

				//affecting the depth masks!
				if(!cur_val)
				{
					//m_image->depthMaskRef()(r,c) = 0;
					m_image->mappedDepthMaskRef()(r,c) =0;
				}
				
				//rgb mask expand the boundary!
				rgb_mask[c] = 255;
				bool boundary=false;
				if(cur_val) //potential mask
				{
					//check whether it is boundary?
					int filter_radius = 2;
					for(int r_d = -filter_radius; r_d <=filter_radius; r_d++)
					{
						if(boundary)
							break;
						int r_n = r+r_d;
						if(r_n <=0 || r_n >= label_row-1)
						{
							boundary = true;
							break;
						}
						for(int c_d = -filter_radius; c_d <= filter_radius; c_d++)
						{
							int c_n = c+c_d;
							if(c_n <=0 || c_n >= label_col -1)
							{
								boundary = true;
								break;
							}
							if(part_index_im(r_n,c_n) != part_index_im(r,c))
							{
								boundary = true;
								break;
							}
						}
						
					}
					if(boundary)
						rgb_mask[c] = 0;
				}

				if(!cur_val || boundary)
				   rgb_mask[c] = 0;
			}
		}
		
		vec_map_index.clear();
		vec_com_count.clear();
		parts_segment_time.stop();
		//imshow("feature rgb mask image-0", rgb_mask_im);
		//cv::Mat3b user_labels;
		//m_image->fillRgbFromUserLabels(user_labels);
		//user_labels.copyTo(m_image->rgbRef());
		//user_labels.release();
		return part_posted_count;
	}

	void RGBDProcessor::imageFeaturesEstimation()
	{
		FeatureSetParams feature_params(getImageFeaturesDetectorType(),getImageFeatureDescriptorType(),true);
		FeatureSet features;
		
		if(!(m_flags & PartsSegment))
			partsSegmentation();
		TimeCount tc_extract("Extract keypoints",0);
		features.extractFromImage(*m_image, feature_params);
		tc_extract.stop();
		ntk_dbg_print(features.locations().size(), 1);
		cv::Mat3b display_image;
		features.draw(m_image->rgb(), display_image);
		imshow("feature points", display_image);
		imshow("feature mask image", m_image->depthMask());
		//imwrite("debug_features.png", display_image);
	}

} // ntk

namespace ntk
{

    RGBDProcessor* RGBDProcessorFactory :: createProcessor(const RGBDProcessorFactory::Params& params)
    {
        RGBDProcessor* processor = 0;
        if (params.camera_type == "kinect-ni")
        {
            processor = new OpenniRGBDProcessor();
        }
        else if (params.camera_type == "kinect-freenect")
        {
            processor = new FreenectRGBDProcessor();
        }
        else
        {
            processor = new RGBDProcessor();
        }

        if (params.do_mapping)
            processor->setFilterFlag(RGBDProcessor::ComputeMapping, true);
        return processor;
    }

}
